package autonomous;

import java.util.ArrayList;

import processing.core.PApplet;
import processing.core.PVector;

public class CrowdBoid {
	// Path Following
	// Daniel Shiffman <http://www.shiffman.net>
	// The Nature of Code, Spring 2009

	// Boid class

	// All the usual stuff
	PVector loc;
	PVector vel;
	PVector acc;
	float r;
	float maxforce; // Maximum steering force
	float maxspeed; // Maximum speed
	PApplet parent; //

	// Constructor initialize all values
	public CrowdBoid(PApplet p, PVector l, float ms, float mf) {
		parent = p;
		loc = l.get();
		r = 12;
		maxspeed = ms;
		maxforce = mf;
		acc = new PVector(0, 0);
		vel = new PVector(maxspeed, 0);
	}

	// A function to deal with path following and separation
	public void applyForces(ArrayList boids, Path path) {
		// Follow path force
		PVector f = follow(path);
		// Separate from other boids force
		PVector s = separate(boids);
		// Arbitrary weighting
		f.mult(3);
		s.mult(1);
		// Accumulate in acceleration
		acc.add(f);
		acc.add(s);
	}

	// Main "run" function
	public void run() {
		update();
		borders();
		render();
	}

	// This function implements Craig Reynolds' path following algorithm
	// http://www.red3d.com/cwr/steer/PathFollow.html
	PVector follow(Path p) {

		// Predict location 25 (arbitrary choice) frames ahead
		PVector predict = vel.get();
		predict.normalize();
		predict.mult(25);
		PVector predictLoc = PVector.add(loc, predict);

		// Draw the predicted location
		// if (debug) {
		parent.fill(0);
		parent.stroke(0);
		parent.line(loc.x, loc.y, predictLoc.x, predictLoc.y);
		parent.ellipse(predictLoc.x, predictLoc.y, 4, 4);
		// }

		// Now we must find the normal to the path from the predicted location
		// We look at the normal for each line segment and pick out the closest
		// one
		PVector target = null;
		PVector dir = null;
		float record = 1000000; // Start with a very high record distance that
								// can easily be beaten

		// Loop through all points of the path
		for (int i = 0; i < p.points.size(); i++) {

			// Look at a line segment
			PVector a = (PVector) p.points.get(i);
			PVector b = (PVector) p.points.get((i + 1) % p.points.size()); // Path
																			// wraps
																			// around

			// Get the normal point to that line
			PVector normal = getNormalPoint(predictLoc, a, b);

			// Check if normal is on line segment
			float da = PVector.dist(normal, a);
			float db = PVector.dist(normal, b);
			PVector line = PVector.sub(b, a);
			// If it's not within the line segment, consider the normal to just
			// be the end of the line segment (point b)
			if (da + db > line.mag() + 1) {
				normal = b.get();
				// If we're at the end we really want the next line segment for
				// looking ahead
				a = (PVector) p.points.get((i + 1) % p.points.size());
				b = (PVector) p.points.get((i + 2) % p.points.size()); // Path
																		// wraps
																		// around
				line = PVector.sub(b, a);
			}

			// How far away are we from the path?
			float d = PVector.dist(predictLoc, normal);
			// Did we beat the record and find the closest line segment?
			if (d < record) {
				record = d;
				// If so the target we want to steer towards is the normal
				target = normal;

				// Look at the direction of the line segment so we can seek a
				// little bit ahead of the normal
				dir = line;
				dir.normalize();
				// This is an oversimplification
				// Should be based on distance to path & velocity
				dir.mult(25);
			}
		}

		// Draw the debugging stuff
		// if (debug) {
		// Draw normal location
		parent.fill(0);
		parent.noStroke();
		parent.line(predictLoc.x, predictLoc.y, target.x, target.y);
		parent.ellipse(target.x, target.y, 4, 4);
		parent.stroke(0);
		// Draw actual target (red if steering towards it)
		parent.line(predictLoc.x, predictLoc.y, target.x, target.y);
		if (record > p.radius)
			parent.fill(255, 0, 0);
		parent.noStroke();
		parent.ellipse(target.x + dir.x, target.y + dir.y, 8, 8);
		// }

		// Only if the distance is greater than the path's radius do we bother
		// to steer
		if (record > p.radius || vel.mag() < 0.1) {
			target.add(dir);
			return steer(target, false);
		} else {
			return new PVector(0, 0);
		}
	}

	// A function to get the normal point from a point (p) to a line segment
	// (a-b)
	// This function could be optimized to make fewer new Vector objects
	PVector getNormalPoint(PVector p, PVector a, PVector b) {
		// Vector from a to p
		PVector ap = PVector.sub(p, a);
		// Vector from a to b
		PVector ab = PVector.sub(b, a);
		ab.normalize(); // Normalize the line
		// Project vector "diff" onto line by using the dot product
		ab.mult(ap.dot(ab));
		PVector normalPoint = PVector.add(a, ab);
		return normalPoint;
	}

	// Separation
	// Method checks for nearby boids and steers away
	PVector separate(ArrayList boids) {
		float desiredseparation = r * 2;
		PVector steer = new PVector(0, 0, 0);
		int count = 0;
		// For every boid in the system, check if it's too close
		for (int i = 0; i < boids.size(); i++) {
			CrowdBoid other = (CrowdBoid) boids.get(i);
			float d = PVector.dist(loc, other.loc);
			// If the distance is greater than 0 and less than an arbitrary
			// amount (0 when you are yourself)
			if ((d > 0) && (d < desiredseparation)) {
				// Calculate vector pointing away from neighbor
				PVector diff = PVector.sub(loc, other.loc);
				diff.normalize();
				diff.div(d); // Weight by distance
				steer.add(diff);
				count++; // Keep track of how many
			}
		}
		// Average -- divide by how many
		if (count > 0) {
			steer.div((float) count);
		}

		// As long as the vector is greater than 0
		if (steer.mag() > 0) {
			// Implement Reynolds: Steering = Desired - Velocity
			steer.normalize();
			steer.mult(maxspeed);
			steer.sub(vel);
			steer.limit(maxforce);
		}
		return steer;
	}

	// Method to update location
	void update() {
		// Update velocity
		vel.add(acc);
		// Limit speed
		vel.limit(maxspeed);
		loc.add(vel);
		// Reset accelertion to 0 each cycle
		acc.mult(0);
	}

	void seek(PVector target) {
		acc.add(steer(target, false));
	}

	void arrive(PVector target) {
		acc.add(steer(target, true));
	}

	// A method that calculates a steering vector towards a target
	// Takes a second argument, if true, it slows down as it approaches the
	// target
	PVector steer(PVector target, boolean slowdown) {
		PVector steer; // The steering vector
		PVector desired = PVector.sub(target, loc); // A vector pointing from
													// the location to the
													// target
		float d = desired.mag(); // Distance from the target is the magnitude of
									// the vector
		// If the distance is greater than 0, calc steering (otherwise return
		// zero vector)
		if (d > 0) {
			// Normalize desired
			desired.normalize();
			// Two options for desired vector magnitude (1 -- based on distance,
			// 2 -- maxspeed)
			if ((slowdown) && (d < 100.0f))
				desired.mult(maxspeed * (d / 100.0f)); // This damping is
														// somewhat arbitrary
			else
				desired.mult(maxspeed);
			// Steering = Desired minus Velocity
			steer = PVector.sub(desired, vel);
			steer.limit(maxforce); // Limit to maximum steering force
		} else {
			steer = new PVector(0, 0);
		}
		return steer;
	}

	void render() {
		// Simpler boid is just a circle
		parent.fill(75);
		parent.stroke(0);
		parent.pushMatrix();
		parent.translate(loc.x, loc.y);
		parent.ellipse(0, 0, r, r);
		parent.popMatrix();
	}

	// Wraparound
	void borders() {
		if (loc.x < -r)
			loc.x = parent.width + r;
		// if (loc.y < -r) loc.y = height+r;
		if (loc.x > parent.width + r)
			loc.x = -r;
		// if (loc.y > height+r) loc.y = -r;
	}

}
